#include "Ui.h"

Ui::Ui(model *mod)
{
    _model = mod;
    cameraIsCentered = false;
    setupCentralWidget();
    setMinimumSize(720,480);
    setWindowTitle(QString("3D - Ardoran"));
    statusBar()->showMessage(tr("Ready"));
}

Ui::~Ui()
{
    delete _model;
    delete _viewer;
}

void Ui::pointPickCallback(const visualization::PointPickingEvent& event, void* cookie)
{
    Ui *ui = (Ui*)cookie;
    float x,y,z;
    if (event.getPointIndex() == -1)
        ui->statusBar()->showMessage(tr("No point was clicked"));
    else
    {
        event.getPoint(x,y,z);
        ui->statusBar()->showMessage(QString("Point Clicked index: %1 x: %2 y: %3 z: %4")
                                 .arg(event.getPointIndex())
                                 .arg(x)
                                 .arg(y)
                                 .arg(z)
                                 );
    }
}

void Ui::setupCentralWidget()
{
    QWidget* mainWidget = new QWidget;
    QVBoxLayout* mainLayout = new QVBoxLayout;

//    QHBoxLayout* loadTargetLayout = new QHBoxLayout;
//    QPushButton* loadTargetButton = new QPushButton(QString("Load Target... "));
//    _loadTargetPath = new QLineEdit(QString("<-- Click here to load the reference cloud"));
//    connect(loadTargetButton, SIGNAL(clicked()), this, SLOT(browseTarget()));
//    loadTargetLayout->addWidget(loadTargetButton);
//    loadTargetLayout->addWidget(_loadTargetPath);

    QHBoxLayout* loadSourceLayout = new QHBoxLayout;
    QPushButton* loadSourceButton = new QPushButton(QString("Load Source..."));
    _loadSourcePath = new QLineEdit(QString("<-- Click here to load the source cloud"));
    connect(loadSourceButton, SIGNAL(clicked()), this, SLOT(browseSource()));
    loadSourceLayout->addWidget(loadSourceButton);
    loadSourceLayout->addWidget(_loadSourcePath);

    QGridLayout* visualizerLayout = new QGridLayout;
    _visualizerQVTKWidget = new QVTKWidget();
    _viewer = new visualization::PCLVisualizer("3DViewer", false);
    _visualizerQVTKWidget->SetRenderWindow(_viewer->getRenderWindow());
    _viewer->setupInteractor(_visualizerQVTKWidget->GetInteractor(), _visualizerQVTKWidget->GetRenderWindow());
    _viewer->getInteractorStyle()->setKeyboardModifier(pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
    _viewer->initCameraParameters();
    _viewer->registerPointPickingCallback(&pointPickCallback, this);
    QSizePolicy buttonPolicy(QSizePolicy::Minimum, QSizePolicy::MinimumExpanding);
    QPushButton* showSourceButton = new QPushButton(QString("Show/Hide Source"));
    showSourceButton->setSizePolicy(buttonPolicy);
    connect(showSourceButton, SIGNAL(clicked()), this, SLOT(showSource()));
    QPushButton* showTargetButton = new QPushButton(QString("Show/Hide Target"));
    showTargetButton->setSizePolicy(buttonPolicy);
    connect(showTargetButton, SIGNAL(clicked()), this, SLOT(showTarget()));
    QPushButton* showRegisteredButton = new QPushButton(QString("Show/Hide Registered"));
    showRegisteredButton->setSizePolicy(buttonPolicy);
    connect(showRegisteredButton, SIGNAL(clicked()), this, SLOT(showRegistered()));
    QPushButton* startButton = new QPushButton(QString("Start"));
    startButton->setSizePolicy(buttonPolicy);
    connect(startButton, SIGNAL(clicked()), this, SLOT(start()));
    QLabel* baseTextLabel = new QLabel(QString("Base check: "));
    _baseIconLabel = new QLabel;
    _baseIconLabel->setPixmap(QPixmap(":/icons/default.png"));
    QLabel* sideTextLabel = new QLabel(QString("Side check: "));
    _sideIconLabel = new QLabel;
    _sideIconLabel->setPixmap(QPixmap(":/icons/default.png"));
    visualizerLayout->addWidget(showSourceButton, 0, 0, 2, 1);
    visualizerLayout->addWidget(showTargetButton, 2, 0, 2, 1);
    visualizerLayout->addWidget(showRegisteredButton, 4, 0, 2, 1);
    visualizerLayout->addWidget(startButton, 6, 0, 2, 1);
    visualizerLayout->addWidget(baseTextLabel, 8, 0);
    visualizerLayout->addWidget(_baseIconLabel, 9, 0);
    visualizerLayout->addWidget(sideTextLabel, 10, 0);
    visualizerLayout->addWidget(_sideIconLabel, 11, 0);
    visualizerLayout->addWidget(_visualizerQVTKWidget, 0, 1, 12, 12);

//    mainLayout->addLayout(loadTargetLayout);
    mainLayout->addLayout(loadSourceLayout);
    mainLayout->addLayout(visualizerLayout);
    mainWidget->setLayout(mainLayout);
    setCentralWidget(mainWidget);
}

// SLOTS
void Ui::browseSource()
{
    QString fileName = QFileDialog::getOpenFileName(this, tr("Load Source Point Cloud"), "/home/membersheep/dataset/2.2 Ardoran", tr("Point Clouds *.pcd (*.pcd);;All Files (*.*)"));
    _loadSourcePath->setText(fileName);
    if(_model->loadSourceCloud(_loadSourcePath->displayText()))
        statusBar()->showMessage(_loadSourcePath->displayText()+QString(" successfully loaded!"));
    else statusBar()->showMessage(QString("Couldn't load the source point cloud, maybe the path or the filename are not correct."));
}

//void Ui::browseTarget()
//{
//    QString fileName = QFileDialog::getOpenFileName(this, tr("Load Target Point Cloud"), "/home/membersheep/dataset/2.2 Ardoran", tr("Point Clouds *.pcd (*.pcd);;All Files (*.*)"));
//    _loadTargetPath->setText(fileName);
//    if(_model->loadTargetCloud(_loadTargetPath->displayText()))
//        statusBar()->showMessage(_loadTargetPath->displayText()+QString(" successfully loaded!"));
//    else statusBar()->showMessage(QString("Couldn't load the target point cloud, maybe the path or the filename are not correct."));
//}

void Ui::showTarget()
{
    if(!_viewer->removePointCloud("target"))
    {
        _viewer->addPointCloud(_model->getTargetCloud(), "target");
        _viewer->addCube(22.0f, 30.0f, 120.0f, 140.0f, 407.0f, 415.0f, 255, 0, 0, "cube1"); // Base cut cube
        _viewer->addCube(-33.0f, -28.0f, 130.0f, 143.0f, 397.0f, 403.0f, 0, 255, 0,"cube2"); // Side cut cube
        if(!cameraIsCentered)
        {
            _viewer->resetCamera();
            cameraIsCentered = true;
        }
        statusBar()->showMessage(QString("Target point cloud added to the visualizer."));
    }
    else
    {
        _viewer->removeAllShapes();
        statusBar()->showMessage(QString("Target point cloud removed from the visualizer."));
    }
    _visualizerQVTKWidget->update();
}

void Ui::showSource()
{
    if(!_viewer->removePointCloud("source"))
    {
        _viewer->addPointCloud(_model->getSourceCloud(), "source");
        if(!cameraIsCentered)
        {
            _viewer->resetCamera();
            cameraIsCentered = true;
        }
        statusBar()->showMessage(QString("Source point cloud added to the visualizer."));
    }
    else statusBar()->showMessage(QString("Source point cloud removed from the visualizer."));
    _visualizerQVTKWidget->update();
}

void Ui::showRegistered()
{
    if(!_viewer->removePointCloud("registered"))
    {
        _viewer->addPointCloud(_model->getRegisteredCloud(), "registered");
        _viewer->addCube(22.0f, 30.0f, 120.0f, 140.0f, 407.0f, 415.0f, 255, 0, 0, "cube3"); // Base cut cube
        _viewer->addCube(-33.0f, -28.0f, 130.0f, 143.0f, 397.0f, 403.0f, 0, 255, 0,"cube4"); // Side cut cube
        if(!cameraIsCentered)
        {
            _viewer->resetCamera();
            cameraIsCentered = true;
        }
        statusBar()->showMessage(QString("Registered point cloud added to the visualizer."));
    }
    else
    {
        _viewer->removeAllShapes();
        statusBar()->showMessage(QString("Registered point cloud removed from the visualizer."));
    }
    _visualizerQVTKWidget->update();
}

void Ui::start()
{
    // TO DO: METTERE L'ESECUZIONE IN UN THREAD SEPARATO
    if(_model->getSourceCloud()->size() == 0 || _model->getTargetCloud()->size() == 0)
        statusBar()->showMessage(QString("Please, load a cloud before executing."));
    else
    {
        statusBar()->showMessage(QString("Computing..."));
//        _model->registerWithLines(_model->getSourceCloud(), _model->getTargetCloud(), _model->getRegisteredCloud(), true);
        registerSourceToTarget(_model->getSourceCloud(), _model->getTargetCloud(), _model->getRegisteredCloud());
        bool baseIsConnected = _model->checkBase(_model->getRegisteredCloud(), _model->getTargetCloud(), Eigen::Vector4f(22,120,407,0), Eigen::Vector4f(30,140,415,0), 80, 2, true);
        bool sideIsConnected = _model->checkSide(_model->getRegisteredCloud(), _model->getTargetCloud(), Eigen::Vector4f(-33,130,397,0), Eigen::Vector4f(-28,143,403,0), 80, 50, true);
        statusBar()->showMessage(QString("Done!"));
        if(baseIsConnected)
            _baseIconLabel->setPixmap(QPixmap(":/icons/correct.png"));
        else _baseIconLabel->setPixmap(QPixmap(":/icons/error.png"));
        if(sideIsConnected)
            _sideIconLabel->setPixmap(QPixmap(":/icons/correct.png"));
        else _sideIconLabel->setPixmap(QPixmap(":/icons/error.png"));

    }
}
